COMMENT ⊗   VALID 00007 PAGES
C REC  PAGE   DESCRIPTION
C00001 00001
C00002 00002	put locations into comtab
C00005 00003	 ROUTINE TO TAKE A MOVE COMMAND GIVEN IN TERMS OF A SERIES OF TRANS ADDRESSES
C00017 00004		RCENTER
C00018 00005	 ROUTINE TO TAKE A DRIVE COMMAND GIVEN IN A RELATIVE OR ABSOLUTE AMOUNT
C00027 00006	FOLLOWING CONTAIN JOINT ANGLES FOR BLUE AND YELLOW ARMS AT THE NEW TRANSES
C00029 00007	ROUTINE TO READ RAW FORCE WRIST AND RETURN INFORMATION TO PDP10
C00031 ENDMK
C⊗;
;put locations into comtab

DATA
PUTLOC	LRPMOVE,RPMOVE
PUTLOC	LRTADRIVE,RTADRIVE
PUTLOC	LRTDDRIVE,RTDDRIVE
PUTLOC	LRCENTER,RCENTER
PUTLOC	LRFORCE, RFORCE
PUTLOC	LMECHNM,MECHNM
CODE

;gets polynomial buffer
;R0=Mechanism word
;it returns the polynomial buffer in R0
GPLYBUF:JSR	PC,MECHNM	; get position of bit
	DEC	R0		; reduce by 1
	ASL	R0		; double it to get the position
	MOV	PLY(R0),R0	; load appropriate address into R0
	RTS	PC

COMMENT ⊗ following routine gives the bit position from the right of the
	first 1; assume at most 6 mechanisms
	⊗
MECHNM:	MOV	R1,-(SP)	; save R1
	MOV	#6,R1		; do 6 times
1$:	BIT	#1,R0		; is rightmost bit on?
	BNE	2$
	ASR	R0		; no, shift
	SOB	R1,1$		; and try again
2$:	MOV	#7,R0		; yes, so complement from 7
	SUB	R1,R0
	MOV	(SP)+,R1	; restore R1
	RTS	PC

DATA
PLY:	.WORD	YPLY,YHPLY,BPLY,BHPLY,VPLY,SPLY
THF:	.WORD	YTHF,YHTHF,BTHF,BHTHF,0,0
PTH:	.WORD	YTH,YHTH,BTH,BHTH,VTH,STH
PJTHF:	.WORD	YJTHF,YHTHF,BJTHF,BHTHF,VTHF,STHF
JTHO:	.WORD	YJTHO,YHJTHO,BJTHO,BHJTHO,VTHO,STHO
MECHN:	.WORD	1,2,4,10,20,40
TMFAC:	.WORD	YTIMFAC,YHTIMFAC,BTIMFAC,BHTIMFAC,0,0
DARRAY:	.WORD	YDARRAY,0,BDARRAY,0,0,0
DDARRAY:.WORD	YDDARRAY,0,BDDARRAY,0,0,0
TMARR:	.WORD	YTMARR,0,BTMARR,0,0,0
TMPTR:	.WORD	YTPTR,0,BTPTR,0,0,0
NUMSEG:	.WORD	0,0,0,0,0,0,0		;number of segments
DURVAL:	.WORD	YDUR,0,BDUR,0,0,0	;total duration
UDURVAL:.WORD	0,0,0,0,0,0		;user duration

YTMARR:	.BLKW	10.		; ten addresses of times
BTMARR:	.BLKW	10.		; ten addresses of times for blue arm
YDUR:	.FLT2	0.0
BDUR:	.FLT2	0.0
CODE
; ROUTINE TO TAKE A MOVE COMMAND GIVEN IN TERMS OF A SERIES OF TRANS ADDRESSES
;AND CONVERTING IT INTO A POLYNOMIAL COEFFICIENT FORM

;DATA LIST STRUCTURES FOR POLYNOMIAL COEFFICIENTS AND DEVICE BLOCKS
;
;THE FOLLOWING DESCRIBES THE REQUIRED ORGANIZATION FOR THE TRANS 
;DATA LIST AND ASSOCIATED SERVO POINTERS:
;
;	SERVO POINTERS		DATA ARRAY
;
;				XXXXXX		TWO SERVO BIT WORDS
;				XXXXXX		  1 BIT FOR EACH SERVO
;				BITS		COMMAND MODE BITS, EG. NNUL,WOBBLE
;				WOBMAG		PTR TO WOBBLE MAGNITUDE CELL
;				DURATION/SPEEDFACTOR *1000
;				TRANS		POINTER TO LIST OF END POINT
;						  TRANSFORMS AND VALIDITY NUMBERS
;				CODE		PTR TO CODE TO BE SCHEDULED THIS SEG
;				TIME		LENGTH OF SEGMENT IN MSECS
;						OR -SPEEDFACTOR*1000
;						0 MEANS SPEEDFACTOR=1 AND NO SPECIFIED TIME
;				TRANS		POINTER TO LIST OF END POINT
;						  TRANSFORMS AND VALIDITY NUMBERS
;				CODE		PTR TO CODE TO BE SCHEDULED THIS SEG
;				TIME		LENGTH OF SEGMENT IN MILLISEC
;				.
;				.
;				.
;				.
;				0
;						END OF MOVE SEGMENTS
;
;
;
;	THE CALLING SEQUENCE FOR THIS ROUTINE IS THE SAME AS IN MOVE
;
;		MOV	#COFLST,R0	;SET POINTER TO DATA LIST
;		MOV	#DEVICE,R1	;STORAGE AREA FOR DEVICE BLOCK,33 WORDS
;					; WHICH WILL BE SAME AS THE ONE PASSED TO MOVE
;		JSR	PC,PMOVE
;		TST	R0		;CHECK FOR ERROR CONDITION
;
;
;	RPMOVE is used when position independent code is desired.
;	(R4) contains the value of the interpreter program counter, and
;	R0 the amount of offset in words of the coeflist	[MSM 1/24/79]
;
;REGISTERS USED
;		R0,R1 PASS ARGUMENTS AND THE STATE IS AS RETURNED FROM MOVE
;		AC0,AC1,AC2,AC3,AC4,AC5 ARE GARBAGED
;
;

CODE
;RELATIVE DISTANCE ACROSS SEGMENTS IN POLY COEF LIST
SGTTME	==4	;OFFSET OF TIME IN MILLISEC FOR THIS SEGMENT
SGTCDE	==2	;OFFSET FOR CODE
SGTTRN	==0	;OFFSET FOR TRANS
SGTNXT	==6	;OFFSET TO NEXT SEGMENT
SEGOFF	==310 	;SPAN OF COEFFICIENTS
A4COEF	==30	;A4 TERM OF FIRST POLYNOMIAL

; dont change value in R4;;;

RPMOVE:
PMOVE:	MOV	R2,-(SP)	;SAVE REGISTERS
	MOV	R3,-(SP)
	MOV	R5,-(SP)
	MOV	R1,-(SP)

	MOV	R0,R1		; save coef list temp
	MOV	@(R4),R0	;mechanism bits, actually @IPC(R4), but IPC=0
	JSR	PC,MECHNM	; get order of mechanism
	DEC	R0
	ASL	R0
	MOV	R0,R5		; get appropriate offset in R5
	MOV	PLY(R5),-(SP)	; polynomial starting address in the stack
	MOV	(SP),-(SP)	; this used as pointer
	MOV	R1,-(SP)	;SAVE FOR LATER USE
				; (SP)=coef list
				; 2(SP)=ptr to polylist
				; 4(SP)= beginning of polylist
;COPY OUT HEADER INFO FROM THE TRANS LIST
	MOV	#4,R3		;COPY FIRST 4 PIECES OF HEADER INFO
	MOV	2(SP),R2	;R2=ptr to polylist
1$:	MOV	(R1)+,(R2)+
	SOB	R3,1$
	MOV	(R1)+,UDURVAL(R5)	; global duration
	MOV	R1,(SP)		;update pointers
	MOV	R2,2(SP)
				;AT THIS POINT (SP) POINTS TO THE TRANSFORM
				;2(SP) DATPT{PTR}
NXTTR0:	MOV	#7,R3		;MOVE CURRENT VALUES OF JOINT ANGLES INTO
	MOV	PTH(R5),R2	;BJTHF AND YJTHF
	MOV	PJTHF(R5),R1
1$:	LDF	@(R2)+,AC0
	STF	AC0,(R1)+
	SOB	R3,1$

	MOV	TMARR(R5),TMPTR(R5);	set up pointers to array of times
	CLR	NUMSEG(R5)	; set up counter for number of segments
	CLRF	@DURVAL(R5)	; set total duration=0

NXTTRN:	INC	NUMSEG(R5)	;one more segment to process
	MOV	#7,R3		;FIRST COPY VALUES AT BEG OF SEG INTO BJTHO
	MOV	JTHO(R5),R0	; 
	MOV	PJTHF(R5),R1	;
1$:	LDF	(R1)+,AC0
	STF	AC0,(R0)+
	SOB	R3,1$
	MOV	@(SP),R0	;SETUP CALL TO SOLVE
	JSR	PC,@LGETARG	;find the address of the transform
	MOV	2(R0),R0	;R0←LOC[trans]
	MOV	#THETFP,R1
	MOV	MECHN(R5),R2
	JSR	PC,SOLVE
	TST	R0		;TEST AND CHECK IF SOLVE DID THE RIGHT THING
	BEQ	11$
	ALERR	NOSOLM		;complain 
	TST	R0
	BEQ	11$		;cleared by user, continue
	MOV	#NOSOLU,R0	;ELSE SIGNAL NO ARM SOLUTION FOUND
	ADD	#6,SP
	MOV	(SP)+,R1	;clear out stack
	JMP	PMDNE		; and exit cleanly
11$:	LDF	FM10,AC3	;AC3←-10
	LDF	FM1P5,AC2
	STF	AC2,AC4		;AC4←-15/10
	LDF	FM0P4,AC2
	STF	AC2,AC5		;AC5←6/(-15)
	CLRF	AC2		;USED TO STORE MAX TIME SO FAR

	MOV	TMFAC(R5),R1	;USED TO GET TIME FACTORS
	MOV	PJTHF(R5),R2	;R2 POINTS TO VALUES AT END OF SEGMENT
	MOV	2(SP),R0	;R0 IS A POINTER THAT OUTPUTS INDIVIDUAL COEF
	ADD	#A0COEF,R0	;SET UP THE VALUE TO OUTPUT
	MOV	#6,R3		;COMPUTE CHANGE IN ANGLES AND OUTPUT THE RELEVANT VALUES
2$:	LDF	BJTHO-BJTHF(R2),AC0
	STF	AC0,(R0)+	;OUTPUT A0
	CLRF	(R0)+		;OUTPUT A1
	CLRF	(R0)+		;OUTPUT A2
	SUBF	(R2)+,AC0	;AC0←BJTHO-BJTHF
	LDF	(R1)+,AC1	;AC1 NOW HAS TIMFAC
	MULF	AC0,AC1		;AC1 NOW HAS TIME TAKEN FOR THIS JOINT
	ABSF	AC1		;TAKE THE ABSOLUTE VALUE
	CMPF	AC1,AC2		;COMPARE THIS VALUE TO VALUE SO FAR
	CFCC			;COPY FLOATING CONDITION CODES
	BLT	5$		;TIME FOR THIS JOINT SMALLER?
	LDF	AC1,AC2		;NO, UPDATE AC2
5$:	MULF	AC3,AC0		;AC0←A3= (BJTHF-BJTHO)*10
	STF	AC0,(R0)+	;OUTPUT A3
	MULF	AC4,AC0		;AC0←A4
	STF	AC0,(R0)+	;OUTPUT A4
	MULF	AC5,AC0		;AC0←A5
	STF	AC0,(R0)+	;OUTPUT A5
	SOB	R3,2$

	MOV	(SP),R3
	LDCIF	SGTTME(R3),AC1	;AC1←time in msecs or speedfactor that user wants
	CFCC
	BLT	6$		; it is speed factor
	MULF	SPEED,AC2	;MULTIPLY BY DEFAULT SPEED FACTOR to get our time
	CMPF	AC1,AC2		; compare user time with ours
	CFCC
	BLT	8$		; users is less, use ours
	STF	AC1,AC2		; users is more, use his
	BR	8$		;AC2 has time in msec
6$:	DIVF	MM1000,AC1	; convert to speed factor
	MULF	AC1,AC2		;AC2←time
8$:	STCFI	AC2,R2
	ADD	#400.,R2	;ADD .4 SEC SLACK TIME
	CMP	#600.,R2	;COMPARE WITH .6 SEC TIME
	BLT	9$
	MOV	#600.,R2	;MIN TIME FOR TRAJ=.6 SEC
9$:	MOV	2(SP),R3
	MOV	R2,SEGTME(R3)	;PUSH INTO THE POLYNOMIAL LIST
	MOV	#SEGOFF,(R3)	;SET UP POINTER TO NEXT SEGMENT POINTER
;	MOV	SGTTRN(R5),SEGTRN(R4);TRANSFER TRANS NUMBER
	CLR	SEGTRN(R3)	;FOR POINTY THIS IS ZERO
;	MOV	SGTCDE(R5),RNCODE(R4)	;RUNCODE OFFSET
	CLR	RNCODE(R3)	;FOR POINTY THIS IS ZERO
	ADD	#SEGTME,R3	;remember pointer to the time
	MOV	R3,@TMPTR(R5)
	ADD	#2,TMPTR(R5)	; and update the pointers
	LDF	@DURVAL(R5),AC0	;AC0←cumulative motion time so far
	LDCIF	R2,AC2		; AC2←time for this segment
	ADDF	AC2,AC0		; add current segment time
	STF	AC0,@DURVAL(R5)	;and update it

; GET READY TO CALL BEJCZY
	MOV	#6,R3		;SETUP UP DARRAY PRIOR TO CALLING BEJCZY
	MOV	DARRAY(R5),R1	;DARRAY IS TO BE SET UP WITH ADDRESSES OF
3$:	MOV	R0,(R1)+	; FOLLOWING
	ADD	#4,R0		;D1,D2,D3,...D6,D11,D22,D33,D44,D55,D66
	MOV	R0,12(R1)
	ADD	#4,R0
	SOB	R3,3$

	MOV     THF(R5),R0	;PTR TO LIST OF PTRS TO JOINT ANGLES
	MOV	DARRAY(R5),R1	;PTR TO LIST OF PTRS TO D(I)'S AND D(II)'S
	MOV	MECHN(R5),R2    ;MECHANISM BIT INDICATING DEVICE TO USE
	JSR	PC,DTERMS		;CALL BEJCZY ROUTINES

	ADD	#SEGOFF,2(SP)	;OFFSET POLYNOMIAL PTR BY RIGHT AMOUNT
	ADD	#SGTNXT,(SP)	;JUMP COEFLIST TO THE NEXT SEGMENT TRANS INFO
	CMP	@(SP),#0
	BEQ	4$		;ANY MORE TRANSES TO COMPUTE?
	JMP	NXTTRN		;YUP, NOT OVER YET
; now compare duration given by user with ours
4$:	LDCIF	UDURVAL(R5),AC0	;AC0←user duration
	CFCC
	BGE	40$		; is not speed factor, so handle accordingly
	DIVF	MM1000,AC0	;AC0←speed factor
	BR	401$
40$:	LDF	@DURVAL(R5),AC1	;AC1←our duration
	CMPF	AC1,AC0
	CFCC
	BGT	42$		; ours is bigger, use it
	DIVF	AC1,AC0		; AC0←stretching factor
401$:	MOV	NUMSEG(R5),R3	; R3←no of segments to process
	MOV	TMARR(R5),R2	; R2 indexes into time list
41$:	LDCIF	@(R2),AC1
	MULF	AC0,AC1		; AC1 is the new time
	STCFI	AC1,@(R2)+	; update the time
	SOB	R3,41$

42$:	TST	(SP)+		; Pop coef list
	CLR	@(SP)+		;OUTPUT FINAL ZERO TO INDICATE NO MORE SEGMENTS
	MOV	(SP)+,R0	;SETUP TO CALL MOVE
	MOV	(SP)+,R1	;GET THE DEVICE BLOCK ADRESS
	JSR	PC,MOVE		;WAVE THE ARM
PMDNE:	MOV	(SP)+,R5	;RESTORE REGISTERS
				;R0,R1 WILL BE IN THE STATE IN WHICH THEY LEFT MOVE
	MOV	(SP)+,R3
	MOV	(SP)+,R2
	RTS	PC		;EXIT
;	RCENTER

RCENTER:JMP	CENTER

; ROUTINE TO TAKE A DRIVE COMMAND GIVEN IN A RELATIVE OR ABSOLUTE AMOUNT
;AND CONVERTING IT INTO A POLYNOMIAL COEFFICIENT FORM

;DATA LIST STRUCTURES FOR POLYNOMIAL COEFFICIENTS AND DEVICE BLOCKS
;
;THE FOLLOWING DESCRIBES THE REQUIRED ORGANIZATION FOR THE TRANS 
;DATA LIST AND ASSOCIATED SERVO POINTERS:
;
;	SERVO POINTERS		DATA ARRAY
;
;				XXXXXX		TWO SERVO BIT WORDS
;				XXXXXX		  1 BIT FOR EACH SERVO
;				BITS		COMMAND MODE BITS, EG. NNUL,WOBBLE
;				WOBMAG		PTR TO WOBBLE MAGNITUDE CELL
;			        SAMOUNT		POINTER TO A SCALAR
;				CODE		PTR TO CODE TO BE SCHEDULED THIS SEG
;				TIME		LENGTH OF SEGMENT IN MSECS
;						OR -SPEEDFACTOR*1000
;						0 MEANS SPEEDFACTOR=1 AND NO SPECIFIED TIME
;				0
;						END OF MOVE SEGMENTS
;
;	THE CALLING SEQUENCE FOR THIS ROUTINE IS THE SAME AS IN MOVE
;
;		MOV	#COFLST,R0	;SET POINTER TO DATA LIST
;		MOV	#DEVICE,R1	;STORAGE AREA FOR DEVICE BLOCK,33 WORDS
;					; WHICH WILL BE SAME AS THE ONE PASSED TO MOVE
;		JSR	PC,TMOVE
;		TST	R0		;CHECK FOR ERROR CONDITION
;
;
;REGISTERS USED
;		R0,R1 PASS ARGUMENTS AND THE STATE IS AS RETURNED FROM MOVE
;		AC0,AC1,AC2,AC3,AC4,AC5 ARE GARBAGED
;
;


;RELATIVE DISTANCE ACROSS SEGMENTS IN POLY COEF LIST
SGJTME	==4	;OFFSET OF TIME IN MILLISEC FOR THIS SEGMENT
SGJCDE	==2	;OFFSET FOR CODE
SGJTRN	==0	;OFFSET FOR TRANS
SGJOFF	==40. 	;SPAN OF COEFFICIENTS
SGJDAT  ==8.	;BEGINNING OF DATA FROM COEF LIST


RTADRIVE:
;	ASL	R0
;	ADD	(R4),R0
TADRIVE: MOV	#1,ABSDRV	;SET UP FIRST PART OF HEADER INFO
	BR	PDRIVE

RTDDRIVE:
;	ASL	R0
;	ADD	(R4),R0
TDDRIVE: MOV	#0,ABSDRV

PDRIVE:	MOV	R2,-(SP)	;SAVE REGISTERS(1)
	MOV	R3,-(SP)	;(2)
	MOV	R4,-(SP)	;(3)
	MOV	R5,-(SP)	;(4)
	MOV	R1,-(SP)	;(5)

	MOV	R0,R5		;USE R5 AS POINTER TO DATA LIST
	MOV	@(R4),R0	;mechanism bits, actually @IPC(R4), but IPC=0
	JSR	PC,GPLYBUF	; get appropriate address in R0
	MOV	R0,R4		;USE R4 AS PTR TO POLY LIST FOR MOVE
	MOV	R0,-(SP)	;USE R4 AS PTR TO POLY LIST FOR MOVE

	MOV	#4,R3		;COPY FIRST 4 PIECES OF HEADER INFO
1$:	MOV	(R5)+,(R4)+
	SOB	R3,1$
	MOV	R4,-(SP)	;SAVE POINTER FOR FUTURE USE(6)

	MOV	(R0),R2		;GET THE SERVO BITS TO DETERMINE # OF REQ. SRV.
       	MOV	2(R0),R3	;32 POSSIBLE SERVOS
	MOV	#-1,R4
11$:	INC	R4		;INCREMENT SERVO COUNT
	ASHC	#1,R2		;GET  NEXT SERVO BIT
	BNE	11$		;REPEAT TILL ALL SERVO BITS ADDED
	ADD	R4,R4		;DOUBLE TO GET THE OFFSET

	MOV	#7,R3		;MOVE CURRENT VALUES OF JOINT ANGLES INTO
	MOV	#BTH,R2		;BJTHF AND YJTHF
	MOV	#BJTHF,R1
2$:	LDF	@(R2)+,AC0
	STF	AC0,(R1)+
	SOB	R3,2$

;	LDF	SGJTRN(R5),AC1	;NEW ANGLE VALUE STATMENT NOW CHANGED
	MOV	SGJTRN(R5),R0
	MOV	R4,-(SP)	;(7)
	MOV	12(SP),R4
	JSR	PC,@LGETARG
	MOV	(SP)+,R4	;(6)
	MOV	2(R0),R0	; get address of scalar
	LDF	(R0),AC1	;new angle value
	LDF	@THETFP(R4),AC0	;OLD ANGLE VALUE
	CMP	#1,ABSDRV	;ABSOLUTE OR RELATIVE MOVE?
	BEQ	13$
	ADDF	AC0,AC1
13$:				; AC1 now contains the final value
	MOV	R4,-(SP)	; save R4(7)
	MOV	SERVOS(R4),R4	; R4←starting of servo data blocks
	LDF	USTOP(R4),AC2	; AC0←upper limit
	CMPF	AC2,AC1		;
	CFCC
	BGE	131$		; less than upper limit, branch
	STF	AC2,AC1		; otherwise use upper stop
	BR	134$
131$:	MOV	(SP),R4		;(7)
	MOV	SERVOS(R4),R4
	LDF	LSTOP(R4),AC2	;AC0←LOWER LIMIT
	CMPF	AC2,AC1
	CFCC
	BLE	135$		; its OK can proceed
	STF	AC2,AC1		; otherwise use lower stop
134$:	MOV	(SP)+,R1	; get joint no in bytes(6)
	MOV	R1,R4
	ASR	R1
	INC	R1
	MOV	#1,R0		; 
	BR	1341$
1342$::	ASL	R0
1341$::	SOB	R1,1342$
	ALERR	NOSOLM		; print error message
	TST	R0
	BEQ	136$
	MOV	#NOSOLU,R0
	ADD	#4,SP		; clean off top two elements in stack
	BR	DRVDNE		; exit cleanly
135$:	MOV	(SP)+,R4	; (6)
136$::	STF	AC1,@THETFP(R4)	;NOW WE HAVE STORED NEW JOINT VALUE IN RELEVANT SPOT
	MOV	#DDARRAY,R0	;LOAD UP RELEVANT DARRAY COEF
	MOV	#DARRAY,R1
	MOV	#12.,R3
10$:	MOV	R0,(R1)+
	ADD	#4,R0
	SOB	R3,10$		;DARRAY NOW CONTAINS DUMMY ARRAY

	MOV	(SP)+,R1	;GET BACK POINTER TO POLYNOMIAL POSITION(5)
	MOV	#SGJOFF,(R1)	;OUTPUT NEXT SEGMENT POINTER
	MOV	#0,SEGTRN(R1)	;OUTPUT ZERO FOR TRANS NUM
	MOV	#0,RNCODE(R1)	;OUTPUT ZERO FOR CODES NUM
	MOV	SGJTME(R5),SEGTME(R1)	;
	MOV	#3000.,SEGTME(R1)	; give it arbitrary 3 second motion
	ADD	#A0COEFF,R1	;GET POSITION OF POLYNOOMIAL
	STF	AC0,(R1)+	;OUTPUT A0
	CLRF	(R1)+		;A1=0
	CLRF	(R1)+		;A2=0
	SUBF	AC1,AC0		;AC0=OLD-NEW
	MULF	FM10,AC0
	STF	AC0,(R1)+	;A3
	MULF	FM1P5,AC0	;
	STF	AC0,(R1)+	;A4
	MULF	FM0P4,AC0	;
	STF	AC0,(R1)+	;A5

	CMP	#10.,R4		;DETERMINE DEVICE
	BLT	3$
	MOV	#YELARM,R2	;IS YELLOW ARM
	BR	8$
3$:	CMP	#12.,R4
	BLT	4$
	MOV	#YELHND,R2	;IS YELLOW HAND
	MOV	R1,DARRAY
	ADD	#4,R1
	MOV	R1,DARRAY+2
	BR	7$
4$:	CMP	#24.,R4
	BLT	5$
	MOV	#BLUARM,R2	;IS BLUE ARM
	SUB	#16,R4		;CORRECT FOR OFFSET
	BR	8$
5$:	MOV	#BLUHND,R2	;IS BLUE HAND
	MOV	R1,DARRAY
	ADD	#4,R1
	MOV	R1,DARRAY+2
	BR	7$
8$:	MOV	R1,DARRAY(R4)
	ADD	#4,R1
	MOV	R1,DARRAY+14(R4)
7$:	CLR	(R1)+
	CLR	(R1)+
	MOV	#DARRAY,R1
	MOV	#BTHF,R0
	JSR	PC,DTERMS	;MAKE A CALL TO BEJCZY

	MOV	(SP)+,R0	;SETUP TO CALL MOVE
	MOV	(SP)+,R1	;GET THE DEVICE BLOCK ADRESS(4)
	JSR	PC,MOVE		;WAVE THE ARM
DRVDNE:	MOV	(SP)+,R5	;RESTORE REGISTERS(3)
	MOV	(SP)+,R4	;R0,R1 WILL BE IN THE STATE IN WHICH THEY LEFT MOVE(2)
	MOV	(SP)+,R3	;(1)
	MOV	(SP)+,R2	;(0)
	RTS	PC		;EXIT
;FOLLOWING CONTAIN JOINT ANGLES FOR BLUE AND YELLOW ARMS AT THE NEW TRANSES
DATA
YBJTHF:
YJTHF:	.BLKW	16
BJTHF:	.BLKW	16
YJTHO:	.BLKW	16
BJTHO:	.BLKW	16
THETFP:
YTHF:	YJTHF
	YJTHF+4
	YJTHF+10
	YJTHF+14
	YJTHF+20
	YJTHF+24
YHTHF:	YJTHF+30
BTHF:	BJTHF
	BJTHF+4
	BJTHF+10
	BJTHF+14
	BJTHF+20
	BJTHF+24
BHTHF:	BJTHF+30

FM10:	.FLT2	-10.0
FM1P5:	.FLT2	-1.5
FM0P4:	.FLT2	-0.4
BDARRAY:.BLKW	14		;PUT POINTERS TO BEJCZY TERMS
YDARRAY:.BLKW	14		;PUT POINTERS TO BEJCZY TERMS
BDDARRAY: .BLKW	30		;ROOM FOR DUMMY BEJCZY COEFFS THAT ARE NOT NEEDED
YDDARRAY: .BLKW	30		;ROOM FOR DUMMY BEJCZY COEFFS THAT ARE NOT NEEDED
MM1000:	.FLT2	-1000.0
;	 0.75,0.5,4.5,0.2,0.2,0.4,5.0,		! Yellow joint times  FROM ARMSOL
;	 0.75,0.75,4.5,0.2,0.2,0.4,5.0;		! Blue joint times IN JIF/DEG
; SO THE FOLLOWING ARE 1000/60 OF THESE VALUES TO GIVE MSEC/DEG
TIMFAC:
YTIMFAC: .FLT2	12.5
	 .FLT2	8.333
	 .FLT2	75.0
	 .FLT2	3.333
	 .FLT2	3.333
	 .FLT2	6.667
YHTIMFAC:.FLT2	83.333
BTIMFAC: .FLT2	12.5
	 .FLT2	12.5
	 .FLT2	75.0
	 .FLT2	3.333
	 .FLT2	3.333
	 .FLT2	6.667
BHTIMFAC:.FLT2	83.333
SPEED:	 .FLT2	1.0			;DEFAULT SPEED FACTOR
ABSDRV:	1
BPLY:	.BLKW	1000
YPLY:	.BLKW	1000
BHPLY:	.BLKW	100
YHPLY:	.BLKW	100
VPLY:	.BLKW	100
SPLY:	.BLKW	100
CODE
;ROUTINE TO READ RAW FORCE WRIST AND RETURN INFORMATION TO PDP10
; copied over from old POINTY file
; note that the data sent is in integer format
;
; procedure should be called with R1 containing a pointer to
; the buffer in which the data is to be stored.
; sample call is as follows:
;
;	MOV	#FPPTR,R1
;	JSR	PC,RFORCE
;
;	R0,R2 will be garbaged.
;	FPPTR will be updated
;
RFORCE:	MOV	R3,-(SP)	;save registers
	MOV	R4,-(SP)
	MOV	R1,-(SP)
	MOV	(R1),R1		;now R1 has address instead of address of address

	MOV	#10.,R2		;READ 10 SETS OF DATA

SETLP:	MOV	#9.,R3		;EIGHT STRAIN GAGES IN ALL+REF CHAN
	MOV	#30,R4		;FIRST STRAIN GAGE CHANNEL
REDLP:	MOVB	R4,ADCCHN	;START CONVERTING STRAIN GAGE READING
WLP:    TSTB	ADCSR		;WAIT TILL CONVERSION COMPLETED
	BMI	CNVDNE  
	BR 	WLP
CNVDNE:	MOV	ADCVAL,R0	;GET READING FROM BLUE INTERFACE
;	ADD	#2048.,R0
	MOV	R0,(R1)+	;SAVE READING
	INC	R4		;POINT TO NEXT CHANNEL
	SOB	R3,REDLP	;REPEAT UNTIL DONE
	CLR	-2(R1)		;NO REFERENCE READING
	SOB	R2,SETLP	;DO IT 10. TIMES

	MOV	(SP)+,R0	;now update the pointer value
	MOV	R1,(R0)
	MOV	(SP)+,R4	;retrieve registers
	MOV	(SP)+,R3
	RTS	PC